#include "FreeRTOS.h"
#include "queue.h"
#include "string.h"
#include "stdio.h"
#include "stdarg.h"
#include "printf_rtos.h"
extern QueueHandle_t log_queue;
#define printf printf_rtos
#include "usart.h"
void printf_rtos(const char *format,...)
{
    va_list arg1, arg2;
    char *log_buf = NULL;
    int len = 0;

    // --- 第一次调用 vsnprintf，为了获取所需长度 ---
    // 通过传入 NULL 缓冲区和 0 大小，vsnprintf 不会写入任何数据，
    // 而是仅仅返回格式化后的字符串所需的全长。
    va_start(arg1, format);
    len = vsnprintf(NULL, 0, format, arg1);
    va_end(arg1);

    // 如果长度有效
    if (len > 0)
    {
        // --- 根据实际长度，精确分配堆内存 ---
        // len 是字符数，+1 是为结尾的 '\0'
        log_buf = pvPortMalloc(len + 1);
        if (log_buf != NULL)
        {
            // --- 第二次调用 vsnprintf，这次真正地格式化到堆缓冲区 ---
            va_start(arg2, format);
            vsnprintf(log_buf, len + 1, format, arg2);
            va_end(arg2);

            // --- 发送到队列，失败则释放 ---
            // 这里的超时时间可以根据需求调整，例如 pdMS_TO_TICKS(20)
            if (xQueueSend(log_queue, &log_buf, 0) != pdPASS)
            {
                vPortFree(log_buf);
            }
        }
        // 如果 pvPortMalloc 失败，则日志无法发送，但不会崩溃
    }
}

int _write(int fd, char *ptr, int len) {
    HAL_UART_Transmit(&huart1, (uint8_t*)ptr, len, 0xFFFF);
    return len;
}